#include "windmill.hpp"
using namespace std;
using namespace cv;

#define Black Scalar(0,0,0)
#define Red Scalar(0,0,255)
#define Green Scalar(0,255,0)
#define Blue Scalar(255,0,0)

inline double get_dis(Point2f a,Point2f b){
    return sqrt(1.0*(a.x-b.x)*(a.x-b.x)+1.0*(a.y-b.y)*(a.y-b.y));
}

inline double get_angle(Point2f a,Point2f b){
    double dy=a.x-b.x,dx=a.y-b.y;
    double angle=atan(dy/dx);
    if(dx<0 && dy>0)angle+=3.1415926;
    if(dx<0 && dy<0)angle+=3.1415926;
    if(dx>0 && dy<0)angle+=6.2831852;
    return angle;
}

inline double deta_a(double a,double b){
    double ans=abs(a-b);
    if(ans>3.1415926)return 6.2831852-ans;
    return ans;
}

Point draw_xOy(Mat &g){
    int X=g.cols-20,Y=g.rows-20;
    Point O=Point(20,Y);
    circle(g,O,3,Black,-1);
    line(g,O,Point(20,20),Black,2);
    line(g,Point(10,30),Point(20,20),Black,2);
    line(g,Point(30,30),Point(20,20),Black,2);
    line(g,O,Point(X,Y),Black,2);
    line(g,Point(X-10,Y-10),Point(X,Y),Black,2);
    line(g,Point(X-10,Y+10),Point(X,Y),Black,2);
    return O;
}

void draw_point(Mat &g,Point O,vector<pair<double,double> > &p){
    for(int i=0;i<p.size();i++){
        double x=p[i].first,y=p[i].second;
        circle(g,O+Point(x*110,-300*y),3,Blue,-1);
    }
}

void draw_sin(Mat &g,Point O,double A,double W,double phi,double b){
    for(int i=O.x;i<=g.cols-30;i++)
        circle(g,Point(i,O.y-300*(A*sin(W*(i-O.x)/110+phi)+b)),3,Red,-1);
}

int main()
{
    //==========================代码区========================//
    std::chrono::milliseconds lst,t,start = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
    WINDMILL::WindMill wm(start.count());
    cv::Mat src;

    bool flg=true;
    double angle,lst_angle=0;
    unsigned int tot=0;
    
    vector<pair<double,double> > point;
    while (1)
    {
        tot++;
        t = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
        src = wm.getMat((double)t.count()/1000);
        Point2f center,target[5];
        Mat img=src;
        cvtColor(src,img,COLOR_BGR2GRAY);
        threshold(img,img,50,255,THRESH_BINARY);    //二值化
        vector<vector<Point> > contours;
        vector<Vec4i> hierarchy;
        findContours(img,contours,hierarchy,RETR_TREE,CHAIN_APPROX_SIMPLE); //找轮廓
        int cnt=1;
        for(int i=0;i<contours.size();i++){
            if(hierarchy[i][3]==-1){
                double s=contourArea(contours[i]);
                double c=arcLength(contours[i],1);
                if(s/(c*c)<0.025){
                    RotatedRect rect=minAreaRect(contours[hierarchy[i][2]]);
                    circle(src,rect.center,5,Blue,-1);      //标记打击目标
                    target[0]=rect.center;
                }else if(s/(c*c)>0.03){
                    RotatedRect rect=minAreaRect(contours[hierarchy[i][2]]);
                    target[cnt++]=rect.center;
                }
            }
        }
        center=Point(0,0);
        for(int i=0;i<5;i++)
            center+=target[i];
        center/=5;
        circle(src,center,4,Green,-1);          //锁定圆心
        double r=get_dis(center,target[0]);
        circle(src,center,r,Green,1);           //绘制旋转轨迹

        angle=get_angle(target[0],center);
        if(flg){
            lst=t;
            lst_angle=angle;
            flg=false;
            continue;
        }
        if(tot%8==0){
            double w,da=deta_a(angle,lst_angle),dt=t.count()-lst.count();
            w=da*1000/dt;       //计算角速度
            point.push_back(make_pair((lst.count()-start.count())/1000.0,w));   //取点
            lst=t;
            lst_angle=angle;
        }
        imshow("result",src);
        int key=waitKey(1);
        if(key=='a')break;      //手动终止
        if(tot==3600)break;     //取450个点
    }
    double A=1,phi=1,W=2,b=1;       //拟合（梯度下降）
    const int times=100000;
    const double eps=0.0005;
    int N=point.size();
    for(int i=0;i<times;i++){
        double dA=0,dphi=0,dW=0,db=0;
        for(int j=0;j<N;j++){
            double x=point[j].first,y=point[j].second;
            dA+=2.0/N*(A*sin(W*x+phi)+b-y)*sin(W*x+phi);
            dW+=2.0/N*(A*sin(W*x+phi)+b-y)*A*x*cos(W*x+phi);
            dphi+=2.0/N*(A*sin(W*x+phi)+b-y)*A*cos(W*x+phi);
            db+=2.0/N*A*sin(W*x+phi)+b-y;
        }
        A-=eps*dA;
        W-=eps*dW;
        phi-=eps*dphi;
        b-=eps*db;
    }
    cout<<"A="<<A<<endl;        //拟合结果
    cout<<"W="<<W<<endl;
    cout<<"phi="<<phi<<endl;
    cout<<"b="<<b<<endl;
    Mat graph(Size(1500,800),CV_8UC3,Scalar(255,255,255));  //画图
    Point O=draw_xOy(graph);
    draw_point(graph,O,point);
    draw_sin(graph,O,A,W,phi,b);
    imshow("graph",graph);
    waitKey();

    //=======================================================//
}